// --------------------------------------------------------------------------------------------------------------------
// <copyright file="KalmanFilterTest.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Test.Robotics.Numerics
{
    using System;

    using Microsoft.Robotics.Numerics;
    using Microsoft.VisualStudio.TestTools.UnitTesting;

    /// <summary>
    /// The Kalman filter test class
    /// </summary>
    [TestClass]
    public class KalmanFilterTest
    {
        /// <summary>
        /// Validates the size of input array
        /// </summary>
        /// <param name="meanOld">The old mean</param>
        /// <param name="varianceOld">The variance</param>
        /// <param name="u">The control</param>
        /// <param name="z">The measurement</param>
        /// <param name="paramA">The parameters A</param>
        /// <param name="paramB">The parameters B</param>
        /// <param name="paramC">The parameters C</param>
        /// <param name="paramR">The parameters R</param>
        /// <param name="paramQ">The parameters Q</param>
        /// <param name="n">The dimension of state</param>
        /// <param name="m">The dimension of control</param>
        /// <param name="k">The dimension of measurement</param>
        /// <returns>True if the dimensions match, false otherwise</returns>
        public static bool ValidateInputArraySize(
            double[] meanOld, 
            double[,] varianceOld, 
            double[] u, 
            double[] z, 
            double[,] paramA, 
            double[,] paramB, 
            double[,] paramC, 
            double[,] paramR, 
            double[,] paramQ, 
            int n, 
            int m, 
            int k)
        {
            bool isValid = true;
            if (meanOld.Length != n || varianceOld.GetLength(0) != n || varianceOld.GetLength(1) != n)
            {
                isValid = false;
            }

            if (u.Length != m || z.Length != k)
            {
                isValid = false;
            }

            if (paramA.GetLength(0) != n || paramA.GetLength(1) != n)
            {
                isValid = false;
            }

            if (paramB.GetLength(0) != n || paramB.GetLength(1) != m)
            {
                isValid = false;
            }

            if (paramC.GetLength(0) != k || paramC.GetLength(1) != n)
            {
                isValid = false;
            }

            if (paramR.GetLength(0) != n || paramR.GetLength(1) != n)
            {
                isValid = false;
            }

            if (paramQ.GetLength(0) != k || paramQ.GetLength(1) != k)
            {
                isValid = false;
            }

            return isValid;
        }

        /// <summary>
        /// Validates if value of 1D array matches expected array within threshold
        /// </summary>
        /// <param name="array">The array that is validated</param>
        /// <param name="arrayExpected">The array that is expected</param>
        /// <returns>True if the difference between corresponding array values are within threshold,
        /// False otherwise </returns>
        public static bool Check1DArray(double[] array, double[] arrayExpected)
        {
            bool isValueExpected = true;
            double threshold = 0.001F;
            if (array.Length == arrayExpected.Length)
            {
                for (int i = 0; i < array.Length; i++)
                {
                    if (Math.Abs(array[i] - arrayExpected[i]) > threshold)
                    {
                        isValueExpected = false;
                        break;
                    }
                }
            }
            else
            {
                isValueExpected = false;
            }

            return isValueExpected;
        }

        /// <summary>
        /// Validates if value of 2D array matches expected value within threshold
        /// </summary>
        /// <param name="array">The array that is validated</param>
        /// <param name="arrayExpected">The expected array value</param>
        /// <returns>True if the difference between corresponding array values are within threshold, False otherwise</returns>
        public static bool Check2DMatrix(double[,] array, double[,] arrayExpected)
        {
            bool isValueExpected = true;
            double threshold = 0.001F;
            if (array.GetLength(0) == arrayExpected.GetLength(0) && array.GetLength(1) == arrayExpected.GetLength(1))
            {
                for (int i = 0; i < array.GetLength(0); i++)
                {
                    for (int j = 0; j < array.GetLength(1); j++)
                    {
                        if (Math.Abs(array[i, j] - arrayExpected[i, j]) > threshold)
                        {
                            isValueExpected = false;
                            goto Exit;
                        }
                    }
                }
            }
            else
            {
                isValueExpected = false;
            }

            Exit:
            return isValueExpected;
        }

        /// <summary>
        /// Tests Linear Kalman filter
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void TestKalmanFilterLinear()
        {
            int n = 3;
            int m = 2;
            int k = 4;
            double[] meanOld = new[] { -1.3174, 1.1186, 0.0477 };
            double[] u = { -0.3008, -0.7229 };
            double[] z = { -0.3716, 0.1213, -0.0181, 0.6508 };
            double[] meanNew;
            double[,] varianceNew;
            double[,] varianceOld = { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } };
            double[,] paramR = { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } };
            double[,] paramQ = { { 1, 0, 0, 0 }, { 0, 1, 0, 0 }, { 0, 0, 1, 0 }, { 0, 0, 0, 1 } };
            double[,] paramA = { { 0.1524, 0.9961, 0.1067 }, { 0.8258, 0.0782, 0.9619 }, { 0.5383, 0.4427, 0.0046 } };
            double[,] paramB = { { 0.7749, 0.0844 }, { 0.8173, 0.3998 }, { 0.8687, 0.2599 } };
            double[,] paramC =
                {
                    { 0.8001, 0.2638, 0.5797 }, { 0.4314, 0.1455, 0.5499 }, { 0.9106, 0.1361, 0.1450 }, 
                    { 0.1818, 0.8693, 0.8530 }
                };
            double[] meanNewExpected = { 0.2059, -0.0952, -0.0581 };
            double[,] varianceNewExpected =
                {
                    { 0.5292, -0.1153, -0.1506 }, { -0.1153, 1.1089, -0.4038 }, 
                    { -0.1506, -0.4038, 0.6808 }
                };

            bool isValid = ValidateInputArraySize(
                meanOld, varianceOld, u, z, paramA, paramB, paramC, paramR, paramQ, n, m, k);

            if (!isValid)
            {
                meanNew = null;
                varianceNew = null;
                throw new InvalidOperationException(
                    "Mismatched dimensions for computing prediction based on Kalman filter");
            }

            KalmanFilter.ComputeKalmanFilterLinearOneStep(
                meanOld, 
                varianceOld, 
                u, 
                z, 
                paramA, 
                paramB, 
                paramC, 
                paramR, 
                paramQ, 
                n, 
                m, 
                k, 
                out meanNew, 
                out varianceNew);

            Assert.IsTrue(
                true == Check1DArray(meanNew, meanNewExpected), 
                "Mean after Linear Kalman filter does not match expected mean");
            Assert.IsTrue(
                true == Check2DMatrix(varianceNew, varianceNewExpected), 
                "Variance after Linear Kalman filter does not match expected variance");
        }

        /// <summary>
        /// Tests Kalman EKF Filter
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void TestKalmanFilterEKF()
        {
            int n = 3;
            int m = 2;
            int k = 4;
            double[] meanOld = new[] { -1.3174, 1.1186, 0.0477 };
            double[] u = { -0.3008, -0.7229 };
            double[] z = { -0.3716, 0.1213, -0.0181, 0.6508 };
            double[] meanNew;
            double[,] varianceNew;
            double[,] varianceOld = { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } };
            double[,] paramR = { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } };
            double[,] paramQ = { { 1, 0, 0, 0 }, { 0, 1, 0, 0 }, { 0, 0, 1, 0 }, { 0, 0, 0, 1 } };
            double[,] paramA = { { 0.1524, 0.9961, 0.1067 }, { 0.8258, 0.0782, 0.9619 }, { 0.5383, 0.4427, 0.0046 } };
            double[,] paramB = { { 0.7749, 0.0844 }, { 0.8173, 0.3998 }, { 0.8687, 0.2599 } };
            double[,] paramC =
                {
                    { 0.8001, 0.2638, 0.5797 }, { 0.4314, 0.1455, 0.5499 }, { 0.9106, 0.1361, 0.1450 }, 
                    { 0.1818, 0.8693, 0.8530 }
                };

            PredictStateMeanHelper predictStateMeanHelper = new PredictStateMeanHelper();
            predictStateMeanHelper.InitializePredictionParams(paramB, paramA);

            PredictMeasurementHelper predictObservationHelper = new PredictMeasurementHelper();
            predictObservationHelper.InitializePredictionParams(paramC);
            double[] meanNewExpected = { 0.2059, -0.0952, -0.0581 };
            double[,] varianceNewExpected =
                {
                    { 0.5292, -0.1153, -0.1506 }, { -0.1153, 1.1089, -0.4038 }, 
                    { -0.1506, -0.4038, 0.6808 }
                };

            meanNew = null;
            varianceNew = null;
            KalmanFilter.ComputeKalmanFilterEKFOneStep(
                meanOld, 
                varianceOld, 
                u, 
                z, 
                predictStateMeanHelper.PredictMean, 
                predictObservationHelper.PredictMeasurement, 
                paramA, 
                paramC, 
                paramR, 
                paramQ, 
                n, 
                m, 
                k, 
                out meanNew, 
                out varianceNew);

            Assert.IsTrue(
                true == Check1DArray(meanNew, meanNewExpected), 
                "Mean after Linear Kalman filter does not match expected mean");
            Assert.IsTrue(
                true == Check2DMatrix(varianceNew, varianceNewExpected), 
                "Variance after Linear Kalman filter does not match expected variance");
        }

        /// <summary>
        /// Helper class for predicting state mean
        /// </summary>
        public class PredictStateMeanHelper
        {
            /// <summary>
            ///  The parameters A
            /// </summary>
            private double[,] paramA;

            /// <summary>
            /// The parameters B
            /// </summary>
            private double[,] paramB;

            /// <summary>
            /// Initializes prediction parameters
            /// </summary>
            /// <param name="paramBOther">The params B</param>
            /// <param name="paramAOther">The params A</param>
            public void InitializePredictionParams(double[,] paramBOther, double[,] paramAOther)
            {
                this.paramB = new double[paramBOther.GetLength(0), paramBOther.GetLength(1)];
                Array.Copy(paramBOther, this.paramB, this.paramB.Length);
                this.paramA = new double[paramAOther.GetLength(0), paramAOther.GetLength(1)];
                Array.Copy(paramAOther, this.paramA, this.paramA.Length);
            }

            /// <summary>
            /// Predicts mean
            /// </summary>
            /// <param name="u">The control</param>
            /// <param name="meanOld">The old mean</param>
            /// <returns>The predicted mean based on control</returns>
            public double[] PredictMean(double[] u, double[] meanOld)
            {
                double[] temp1 = this.paramA.Multiply(meanOld);
                double[] temp2 = this.paramB.Multiply(u);

                double[] meanTemp = temp1.Add(temp2);
                return meanTemp;
            }
        }

        /// <summary>
        /// Class for predicting measurement from predicted mean of state
        /// </summary>
        public class PredictMeasurementHelper
        {
            /// <summary>
            /// The parameters used for prediction
            /// </summary>
            private double[,] paramC;

            /// <summary>
            /// Initializes parameters used for prediction
            /// </summary>
            /// <param name="srcParamC">The parameters</param>
            public void InitializePredictionParams(double[,] srcParamC)
            {
                this.paramC = new double[srcParamC.GetLength(0), srcParamC.GetLength(1)];
                Array.Copy(srcParamC, this.paramC, this.paramC.Length);
            }

            /// <summary>
            /// Predicts measurement from predicted mean of state
            /// </summary>
            /// <param name="predTemp">The predicted mean of state</param>
            /// <returns>The predicted measurement</returns>
            public double[] PredictMeasurement(double[] predTemp)
            {
                double[] measurementPred = this.paramC.Multiply(predTemp);
                return measurementPred;
            }
        }
    }
}
